package es.uji.viselab.math;

import java.io.IOException;
import java.io.Serializable;
import java.nio.DoubleBuffer;

import javax.vecmath.*;

public class Matrix4 extends Matrix4d implements Serializable {
	/**
	 * 
	 */
	private static final long serialVersionUID = 1L;

	public Matrix4(double[][] m) {
		super(m[0][0], m[0][1], m[0][2], m[0][3], m[1][0], m[1][1], m[1][2], m[1][3], m[2][0],
				m[2][1], m[2][2], m[2][3], m[3][0], m[3][1], m[3][2], m[3][3]);
	}

	public Matrix4(double[] m) {
		super(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12],
				m[13], m[14], m[15]);
	}

	public Matrix4() {
	}

	public static Matrix4 getIdentityM() {
		double[][] m = new double[4][4];
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				if (i != j)
					m[i][j] = 0;
				else
					m[i][j] = 1;
			}
		}
		return new Matrix4(m);
	}

	public static double[][] getIdentity() {
		double[][] m = new double[4][4];
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				if (i != j)
					m[i][j] = 0;
				else
					m[i][j] = 1;
			}
		}
		return m;
	}

	/*
	 * public static double[][] rotY(double[][] m4, double alfa) { double[][] m3
	 * = M4.extractM3(m4); double[][] rot = M3.rotY(m3, alfa); m3 = M3.mult(rot,
	 * m3); m4 = M4.set(m4, m3); return m4; }
	 * 
	 * private static double[][] set(double[][] m4, double[][] m3) { for (int i
	 * = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { m4[i][j] = m3[i][j]; } }
	 * return m4;
	 * 
	 * }
	 * 
	 * private static double[][] extractM3(double[][] m4) { double[][] m3 = new
	 * double[3][3]; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) {
	 * m3[i][j] = m4[i][j]; } } return m3; }
	 * 
	 * public static double[][] translate(double[][] a, double x, double y,
	 * double z) { double[][] b = M4.getTranslation(x, y, z); return M4.mult(a,
	 * b); }
	 * 
	 * public static double[][] mult(double[][] b, double[][] a) { // B * A
	 * double[][] r = new double[4][4]; for (int i = 0; i < 4; i++) for (int j =
	 * 0; j < 4; j++) { r[i][j] = 0; for (int k = 0; k < 4; k++) r[i][j] =
	 * r[i][j]+b[i][k] * a[k][j]; } return r; }
	 * 
	 * private static double[][] getZero() { double[][] r = new double[4][4];
	 * for (int i = 0; i < 4; i++) { for (int j = 0; j < 4; j++) { r[i][j] = 0;
	 * } } return r; }
	 * 
	 * public static double[][] getTranslation(double x, double y, double z) {
	 * double[][] a = M4.getIdentity(); a[0][3] = x; a[1][3] = y; a[2][3] = z;
	 * return a; }
	 * 
	 * public static double[] getPosition(double[][] m) { double[] v = {
	 * m[0][3], m[1][3], m[2][3] }; return v; }
	 * 
	 * public static double[] getYPR(double[][] m) { double yaw, pitch, roll;
	 * yaw = Math.atan(m[1][0] / m[0][0]); pitch = Math.atan(-m[2][0] /
	 * Math.sqrt(Math.pow(m[2][1], 2) + Math.pow(m[2][2], 2))); roll =
	 * Math.atan(m[2][1] / m[2][2]); double[] result = { yaw, pitch, roll };
	 * return result; }
	 * 
	 * public static double[][] getRotY(double alfa) { return
	 * rotY(M4.getIdentity(),alfa); }
	 */

	public DoubleBuffer getDoubleBuffer() {
		/*
		 * DoubleBuffer buf = DoubleBuffer.wrap(new double[] { this.m00,
		 * this.m01, this.m02, this.m03, this.m10, this.m11, this.m12, this.m13,
		 * this.m20, this.m21, this.m22, this.m23, this.m30, this.m31, this.m32,
		 * this.m33 });
		 */DoubleBuffer buf = DoubleBuffer.wrap(new double[] { this.m00, this.m10, this.m20,
				this.m30, this.m01, this.m11, this.m21, this.m31, this.m02, this.m12, this.m22,
				this.m32, this.m03, this.m13, this.m23, this.m33 });

		return buf;
	}

	public Vector3 mul(Vector3d v3) {
		Vector4 v4 = new Vector4(v3.x, v3.y, v3.z, 1);
		Vector4 result = this.mul(v4);
		return new Vector3(result.x, result.y, result.z);
	}

	private Vector4 mul(Vector4 v4) {
		return new Vector4(this.m00 * v4.x + this.m01 * v4.y + this.m02 * v4.z + this.m03 * v4.w,
				this.m10 * v4.x + this.m11 * v4.y + this.m12 * v4.z + this.m13 * v4.w, this.m20
						* v4.x + this.m21 * v4.y + this.m22 * v4.z + this.m23 * v4.w, this.m30
						* v4.x + this.m31 * v4.y + this.m32 * v4.z + this.m33 * v4.w);
	}

	public Matrix4 getInverseHomogenousTransformation() {

		// The inverse of a Homogenous Transformation matrix T is:
		Matrix4d m4 = new Matrix4d(this);

		// 3x3 upper left matrix (rotation) = transpose of the upper left part
		// of T
		double[] v3 = { m4.m00, m4.m01, m4.m02, m4.m10, m4.m11, m4.m12, m4.m20, m4.m21, m4.m22 };
		Matrix3d m3 = new Matrix3d(v3);
		m3.transpose();

		// right column (the ones with translation coordinates) =
		// -1 * previous rotation matrix * T(only translation coordinates)
		Matrix3d m3bis = new Matrix3d(m3);
		m3bis.mul(-1);

		// m3bis * m4(only column 3)
		double m03 = m3bis.m00 * m4.m03 + m3bis.m01 * m4.m13 + m3bis.m02 * m4.m23;
		double m13 = m3bis.m10 * m4.m03 + m3bis.m11 * m4.m13 + m3bis.m12 * m4.m23;
		double m23 = m3bis.m20 * m4.m03 + m3bis.m21 * m4.m13 + m3bis.m22 * m4.m23;

		double[][] v4 = { { m3.m00, m3.m01, m3.m02, m03 }, { m3.m10, m3.m11, m3.m12, m13 },
				{ m3.m20, m3.m21, m3.m22, m23 }, { 0, 0, 0, 1 } };
		return new Matrix4(v4);

		/*
		 * Matlab code: function Tinv = invht(T) A = T(1:3,1:3); Tinv = [ A'
		 * -A'*T(1:3,4) 0 0 0 1 ];
		 */
	}

	public void setPosition(double x, double y, double z) {
		m03 = x;
		m13 = y;
		m23 = z;
	}

	public void trans(double x, double y, double z) {
		this.m03 = this.m03 + x;
		this.m13 = this.m13 + y;
		this.m23 = this.m23 + z;

	}

	public static Matrix4 getTranslation(double x, double y, double z) {
		Matrix4 m = Matrix4.getIdentityM();
		m.trans(x, y, z);
		return m;
	}

	public double[] getList() {
		double[] list = { this.m00, this.m01, this.m02, this.m03, this.m10, this.m11, this.m12,
				this.m13, this.m20, this.m21, this.m22, this.m23, this.m30, this.m31, this.m32,
				this.m33 };
		return list;
	}

	public Vector3 getEndEffectorPose() {
		Vector3 v = new Vector3(this.m03, this.m13, this.m23);
		return v;
	}

	

}